//
// Created by yephiu on 2023/7/6.
//

#include "Nizhou_drv_TOF.h"
#include "Ano_FcData.h"
#include "stm32f4xx_hal.h"

uint8_t TOF_STAUTS=TOF_OFFLINE;
float TOF_height_cm=0;
Tofsense TOF_data;

static TOF_DataPacket TOF_received_packet; //声明数据包
static TOF_ParserState TOF_current_state = TOF_WAIT_HEADER; //状态机状态位
uint8_t dataBytesReceived;

//无名TOF使用的波特率为921600
/**
 * @brief TOF初始化
 * @return TOF_STAUTS TOF状态量 ：0 不在线 1 在线 2 测试中
 */
uint8_t TOF_Init(void)
{
    TOF_STAUTS=TOF_TESTTING;
    for (uint16_t i=0;i<100;i++)
    {
        if(TOF_STAUTS==TOF_READY)
        {
            break;
        }
        else
        {
            HAL_Delay(1);//要在100ms内识别到TOF
        }
    }
    if(TOF_STAUTS==TOF_TESTTING)
    {
        TOF_STAUTS=TOF_OFFLINE;
    }
    sens_hd_check.tof_ok=TOF_STAUTS;//将状态更新到fcdata文件中的变量
    return TOF_STAUTS;
}


/**
 * @brief 将24位数据转为int32_t
 * @param data 24bit数据
 * @return
 */
static int32_t NLINK_ParseInt24(TOF_int24_t data)
{
    uint8_t *byte = (uint8_t *)(&data);//取出data指针 并转化为uint8的指针 赋给byte 此时可以视byte为数组
    return (int32_t)(byte[0] << 8 | byte[1] << 16 | byte[2] << 24) / 256;
}

/**
 * 校验和计算函数
 * @param packet 数据包
 * @return
 */
static uint8_t TOF_calculateChecksum(TOF_DataPacket *packet)
{
    uint8_t checksum=0;
    uint8_t *byte_ptr = &(packet->frame_header);

    for (int i = 0; i <sizeof(packet); ++i)
    {
        checksum += byte_ptr[i];
    }

    return checksum & 0xFF;
}

/**
 * TOF_数据解析函数，需要在串口回调函数中被调用
 * @param received_byte 接收到的字节
 */
void TOF_DataAnlTask(uint8_t received_byte)
{

    switch (TOF_current_state)
    {
        case TOF_WAIT_HEADER:
            if (received_byte == 0x57)
            {
                TOF_received_packet.frame_header=received_byte;
                TOF_current_state = TOF_WAIT_FUNCTION_MARK;
            }
            break;
        case TOF_WAIT_FUNCTION_MARK:
            TOF_received_packet.function_mark = received_byte;//接收帧类型
            TOF_current_state = TOF_WAIT_RSERVERD;
            break;
        case TOF_WAIT_RSERVERD:
            TOF_received_packet.reserverd = received_byte;//接收数据长度
            TOF_current_state = TOF_WAIT_ID;
            break;
        case TOF_WAIT_ID:
            TOF_received_packet.ID = received_byte;//接收数据长度
            TOF_current_state = TOF_WAIT_SYSTIME;
            break;
        case TOF_WAIT_SYSTIME:
            ((uint8_t *)TOF_received_packet.system_time)[dataBytesReceived++] = received_byte;//解析浮点数数据
            if (dataBytesReceived >= sizeof(uint32_t))
            {
                dataBytesReceived = 0;
                TOF_current_state = TOF_WAIT_DIS;
            }
            break;
        case TOF_WAIT_DIS:
            ((uint8_t *)(&TOF_received_packet.disatance))[dataBytesReceived++] = received_byte;
            if (dataBytesReceived >= sizeof(TOF_int24_t))
            {
                dataBytesReceived = 0;
                TOF_current_state =TOF_WAIT_DIS_STA;
            }
            break;
        case TOF_WAIT_DIS_STA:
            TOF_received_packet.dis_status=received_byte;
            TOF_current_state =TOF_WAIT_SIGNAL_STRENGTH;
            break;
        case TOF_WAIT_SIGNAL_STRENGTH:
            ((uint8_t *)TOF_received_packet.signal_strength)[dataBytesReceived++] = received_byte;//解析浮点数数据
            if (dataBytesReceived >= sizeof(uint16_t))
            {
                dataBytesReceived = 0;
                TOF_current_state = TOF_WAIT_PRECISION;
            }
            break;
        case TOF_WAIT_PRECISION:
            TOF_received_packet.range_precision=received_byte;
            TOF_current_state =TOF_WAIT_SUMCHECK;
            break;

        case TOF_WAIT_SUMCHECK:
            TOF_received_packet.checksum = received_byte;//接收检验和
            if (TOF_calculateChecksum(&TOF_received_packet) == TOF_received_packet.checksum)//满足校验条件
            {
                TOF_data.ID=TOF_received_packet.ID;
                TOF_data.dis= (float)NLINK_ParseInt24(TOF_received_packet.disatance) / 1000.00f;
                TOF_data.signal_strength=TOF_received_packet.signal_strength;
                TOF_data.status=TOF_received_packet.dis_status;
                // 数据包解析成功，可以在这里处理接收到的数据包
            }
            TOF_STAUTS=TOF_READY;
            TOF_current_state = TOF_WAIT_HEADER;
            break;

        default:
            break;
    }
}

/**
 * 高度数据更新函数 需要在调度器中被周期调用
 * 目前状态未被调用
 */
void TOF_DataUpdateTask(uint8_t dT_ms)
{
    TOF_height_cm=TOF_data.dis;
}
